#include "RotateQuat.h"
#include <string.h>

RotateQuat::RotateQuat(Dof *w, Dof *x, Dof *y, Dof *z){
	mDofs.clear();
	mDofs.resize(4);
	mDofs[0]=w;
	mDofs[1]=x;
	mDofs[2]=y;
	mDofs[3]=z;
	mDofs[0]->setTrans(this);
	mDofs[1]->setTrans(this);
	mDofs[2]->setTrans(this);
	mDofs[3]->setTrans(this);
	mType = Transformation::T_ROTATEQUAT;
}

RotateQuat::RotateQuat(Dof *w, Dof *x, Dof *y, Dof *z, char *_name){
	mDofs.clear();
	mDofs.resize(4);
	mDofs[0]=w;
	mDofs[1]=x;
	mDofs[2]=y;
	mDofs[3]=z;
	mDofs[0]->setTrans(this);
	mDofs[1]->setTrans(this);
	mDofs[2]->setTrans(this);
	mDofs[3]->setTrans(this);
	mType = Transformation::T_ROTATEQUAT;
	strcpy(mName, _name);
}

Mat4d *RotateQuat::getTransformMat(){
	Quaternion q(mDofs[0]->getValue(), mDofs[1]->getValue(),mDofs[2]->getValue(),mDofs[3]->getValue());
	q.normalize();
	
	Mat3d rot = q.getMatrix();
	(*mTransform)=vl_1;
	for(int i=0; i<3; i++){
		for(int j=0; j<3; j++){
            (*mTransform)[i][j] = rot[i][j];
		}
	}
	return mTransform;
}

Mat4d *RotateQuat::getDerivativeMat(Dof *d){
	Quaternion q(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue(), mDofs[3]->getValue());
	q.normalize();	

	Mat3d mat=vl_0;
	(*mDeriv)=vl_0;

	int el=-1;
	for(int i=0; i<4; i++) if(d->getID()==mDofs[i]->getID()) el=i;
	assert(el!=-1);
	mat = q.getDerivativeMatrix(el);
	
	for(int i=0; i<3; i++){
		for(int j=0; j<3; j++){
            (*mDeriv)[i][j] = mat[i][j];
		}
	}

	return mDeriv;
}

Mat4d *RotateQuat::getDerivativeMat(Dof *d1, Dof *d2){
	Quaternion q(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue(), mDofs[3]->getValue());
	q.normalize();
	
	(*mDeriv)=vl_0;
	Mat3d mat=vl_0;
	int el1=-1, el2=-1;
	for(int i=0; i<4; i++) {
		if(d1->getID()==mDofs[i]->getID()) el1=i;
		if(d2->getID()==mDofs[i]->getID()) el2=i;
	}
	assert(el1!=-1);
	assert(el2!=-1);
	mat = q.getDerivativeMatrix(el1, el2);
	for(int i=0; i<3; i++){
		for(int j=0; j<3; j++){
            (*mDeriv)[i][j] = mat[i][j];
		}
	}
	return mDeriv;
}

void RotateQuat::applyGLTransform(){
	Quaternion q(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue(), mDofs[3]->getValue());
	q.normalize();
/*
	mDofs[0]->setValue(q.w);
	mDofs[1]->setValue(q.x);
	mDofs[2]->setValue(q.y);
	mDofs[3]->setValue(q.z);
*/
	Vec3d a = q.getAxis();
	double theta = q.getAngle();
	if(dot(a,a)>EPSILON*EPSILON && theta>EPSILON)
		glRotatef(theta*180/M_PI, a[0], a[1], a[2]);
}
